#include <stereoapp.h>

//
//
// client
//
//

ClientStereoApp::ClientStereoApp(MJPEGStreamer* s)
{
	streamer = s;
	kbkey = 0;
	cam = new StereoCamera(streamer->w, streamer->h);
	smallfont = new GText(0.4,0.4,1);
	expected_frame_duration = AppSettings::instance()->properties->get_float("expected_frame_duration");
	voice_service_port = AppSettings::instance()->properties->get_int("voice_port");
	speaker = new Speaker();
}

char
ClientStereoApp::get_kb_key()
{
	if(kbkey == 27)
	{	
		cam->destroy();
		exit(0);
	}
	return kbkey;
}

IplImage* 
ClientStereoApp::on_get_frame()
{
	char logbuf[256];
	double rest_time;
	
	cam->grab();
	rest_time = MAX(1, cvRound(expected_frame_duration - streamer->frame_duration));
	
	
	sprintf(logbuf,"FPS:%.f, RT:%.f", 
				   1000./(MAX(expected_frame_duration, streamer->frame_duration))
				   ,rest_time);
	smallfont->write(cam->left[RGB],5,20,logbuf, G_LGRAY);
	
	cvShowImage("local image", cam->left[RGB]);
	kbkey = cvWaitKey(rest_time);
	
	return cam->left[GRAY];
}


void 
ClientStereoApp::on_handshake_rcv(HandShakeHeader* header)
{
	// do nothing
	if(!header->active || voice_service_port == 0) return;
	

	
	if(header->enable[0])
	{
		speaker->beeper_enable(0, header->xpos[0]);
	}
	else
	{
		speaker->beeper_disable(0);
	}
	
	if(header->enable[1])
	{
		speaker->beeper_enable(1, header->xpos[1]);
	}
	else
	{
		speaker->beeper_disable(1);
	}
	
	if(header->say)
	{
		speaker->say(header->what);
	} 
	
}

//
//
// server
//
//

ServerStereoApp::ServerStereoApp(MJPEGStreamer* s)
{
	lastkey = 0;
	streamer = s;
	activemenu = AppSettings::instance()->properties->get_int("active_menu");

	tracker = new Tracker(streamer->w, streamer->h);
	pttracker = new PTTracker();
	char* datapath = AppSettings::instance()->properties->get("locator_knodb");
	place_locator = new PlaceLocator(streamer->w, streamer->h, datapath);
	detector = new Detector();
	bp = new BP(streamer->w, streamer->h);
	bp->enablepropagation = AppSettings::instance()->properties->get_int("enable_bp_of_depth");

	left = cvCreateImage(cvSize(streamer->w, streamer->h),8,1);
	left_hold = cvCreateImage(cvSize(streamer->w, streamer->h),8,1);
	right = cvCreateImage(cvSize(streamer->w, streamer->h),8,1);
	right_hold = cvCreateImage(cvSize(streamer->w, streamer->h),8,1);
	hogworker_input = cvCreateImage(cvSize(streamer->w, streamer->h), 8, 1);
	hogworker_output = cvCreateImage(cvSize(streamer->w, streamer->h), 8, 3);
	locatorworker_input = cvCreateImage(cvSize(streamer->w, streamer->h), 8, 1);
	locatorworker_output = cvCreateImage(cvSize(streamer->w, streamer->h), 8, 3);
	detector_output = cvCreateImage(cvSize(streamer->w, streamer->h), 8, 3);
	
	depth_outputBig = cvCreateImage(cvSize(2*streamer->w, 2*streamer->h), 8, 1);
	hogworker_outputBig = cvCreateImage(cvSize(2*streamer->w, 2*streamer->h), 8, 3);
	detector_outputBig = cvCreateImage(cvSize(2*streamer->w, 2*streamer->h), 8, 3);
	
	bpworker = new Worker(this, (void*)bp);
	hogworker = new Worker(this, (void*)tracker);
	locatorworker = new Worker(this, (void*)place_locator);
	
	show_small_images = AppSettings::instance()->properties->get_int("show_small_images");
	depth_th = AppSettings::instance()->properties->get_int("depth_th");
	show_3dviz = AppSettings::instance()->properties->get_int("show_3dviz");
	
	if(show_3dviz)
	{
		navi3d = Navi3D::instance(); 
	}
	
}

void
ServerStereoApp::signal(void* obj)
{
	if(obj == bp)
	{
		bp->process(left_hold, right_hold);
	}
	else if(obj == place_locator)
	{
		this->place_locator->process(hogworker_input);
		if(activemenu == 1)
		{
			this->place_locator->handle_user_input(lastkey);
			lastkey = -1;
		}
	}
	else if(obj == tracker)
	{
		this->tracker->process(hogworker_input);
		if(activemenu == 0)
		{
			this->tracker->handle_user_input(lastkey);
			lastkey = -1;
		}
	}
}
bool plays = 0;
void
ServerStereoApp::on_kb_key(char c)
{
	if(c > 0)
	{
		lastkey = c;
	}
	if(c == 27)
	{
		exit(0);
	}
}

int sayitflag=0;
char whattosay[64];

void 
ServerStereoApp::on_handshake_send(HandShakeHeader* header)
{
	memset(header, 0, sizeof(HandShakeHeader));
	
	// tell client what it sees
	if(sayitflag)
	{
		header->active = 1;
		header->say = 1;
		strcpy(header->what, whattosay);
		printf("say %s bo %d\n", header->what, sayitflag);
		header->pos[0] = this->tracker->objseen_at[1];
		header->pos[1] = this->tracker->objseen_at[2];
		sayitflag = 0;
	}
	
	if(detector->obstac[0].played)
	{
		header->active = 1;
		header->enable[0] = 1;
		header->xpos[0] = detector->obstac[0].x;
	}
	else
	{
		header->active = 1;
		header->enable[0] = 0;
		header->xpos[0] = detector->obstac[0].x;
	}
	
	if(detector->obstac[1].played)
	{
		header->active = 1;
		header->enable[1] = 1;
		header->xpos[1] = detector->obstac[1].x;
	}
	else
	{
		header->active = 1;
		header->enable[1] = 0;
		header->xpos[1] = detector->obstac[1].x;
	}
}

void 
ServerStereoApp::on_show_frame(IplImage* stereo)
{
	cvSetImageROI(stereo, cvRect(0,0,streamer->w, streamer->h));
	cvCopy(stereo, left);
	cvSetImageROI(stereo, cvRect(streamer->w,0,streamer->w, streamer->h));
	cvCopy(stereo, right);
	cvResetImageROI(stereo);
	
	//pttracker->track(left);
	//pttracker->extract(left);
	if( show_3dviz )
	{
		//navi3d->cam_rotate(pttracker->drotation);
	}
	
	if(1)if(bpworker->done)
	{
		cvCvtColor(left, detector_output, CV_GRAY2RGB);
		cvCopy(left,left_hold);
		cvCopy(right,right_hold);
		bpworker->run();
		//detector->detect(bp->dpdepth, detector_output, depth_th);
		cvResize(bp->dpdepth,depth_outputBig);
		//cvResize(detector_output,detector_outputBig);
		
		if(!show_small_images)
		{
			cvShowImage("depth",depth_outputBig);
		//	cvShowImage("obstacles",detector_outputBig);
		}
		else
		{
			cvShowImage("depth",bp->dpdepth);
		//	cvShowImage("obstacles",detector_output);
		}
	}
	
	if(0)if(hogworker->done)
	{
		cvCvtColor(left, hogworker_output, CV_GRAY2RGB);
		cvCopy(left,hogworker_input);
		
		this->hogworker->run();
		this->tracker->render(hogworker_output);
		sayitflag = 0;
		if(tracker->objseen_at[0])
		{
			sayitflag = 1;
			strcpy( whattosay, this->tracker->knn->id2name[(int)this->tracker->objseen_at[4]]);
		}
		cvResize(hogworker_output,hogworker_outputBig);
		
		if(!show_small_images)
		{
			cvShowImage("recognized objects",hogworker_outputBig);
		}
		else
		{
			cvShowImage("recognized objects",hogworker_output);
		}
		
		if(this->tracker->objseen_at[0] && show_3dviz)
		{
			navi3d->isee(this->tracker->objseen_at);
		}
	}
	
	if(0)if(locatorworker->done)
	{
		cvCvtColor(left, locatorworker_output, CV_GRAY2RGB);
		cvCopy(left,locatorworker_input);
		this->locatorworker->run();
		this->place_locator->render(locatorworker_output);
		
		cvShowImage("place_locator",locatorworker_output);
		
		if(this->place_locator->objseen_at[0] && show_3dviz)
		{
			navi3d->isee_landmark(this->place_locator->objseen_at);
		}
	}
	
	char c = cvWaitKey(1);
	on_kb_key(c);
}
